#include "stm32f10x.h" 
#include "IC.h"
#include "Motor.h"
#include "Angle.h"
#define KP 1.0f    
#define KI 0.1f    
#define KD 0.5f    
#define T  0.01f   

float new_error, old_error = 0;  
float integral = 0;              
float result;                    

extern float current_angle;     


void Pid_Function_Wait(float target_angle)
{
    new_error = target_angle - current_angle;  // 目标 - 当前（误差）

   
    float portion = new_error * KP;

    
    integral += new_error * T; 
   
    if (integral > 500 / KI) integral = 500 / KI;  // 500是输出上限100/KI的放大 ,  积分限幅（防止饱和，根据输出范围调整）(参考豆包)
    if (integral < -500 / KI) integral = -500 / KI;
    float integral_term = integral * KI;

   
    float differential = ((new_error - old_error) / T )* KD;


    result = portion + integral_term + differential;

    if (result > 99.0f)
        result = 99.0f;
    else if (result < -99.0f)
        result = -99.0f;

  
    old_error = new_error;

 
    Motor_SetSpeed((int8_t)result);
}


		
void SoftTimer_Init(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
    
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
   
    TIM_TimeBaseInitStructure.TIM_Prescaler = 7200 - 1;  
    TIM_TimeBaseInitStructure.TIM_Period = 100 - 1; 
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStructure);
    TIM_Cmd(TIM1,ENABLE); 
  
}





